% EKF based localization of a robot with GPS only

%clear; clc; close all;

% load Q2_example_gps_only.mat %DOES NOT EXIST
load Q2_gps_only.mat
%load Q2_example_data.mat

T = length(u);
ss = length(x0);
xfilt = x0;
Sigma_filt{1} = Sigma0;
for t=1:T-1
    xfilt(:,t+1) = f_robot(xfilt(:,t), u(:,t), dt);
    A = jacobian_f_robot(xfilt(:,t), u(:,t), dt);
	Sigma_filt{t+1} = A*Sigma_filt{t}*A' + Q*dt;% your code here

    if(y_gps_valid(t+1)) % check if there is a valid gps measurement at time t+1
        ypred = f_gps(xfilt(:,t+1)); % the predicted measurement; your code goes into f_gps.m
        C = jacobian_f_gps(xfilt(:,t+1)); % your code goes into jacobian_f_gps.m
        y = y_gps(:,t+1); % the measurement
        
        % some code here:
		K = Sigma_filt{t+1}*C'*inv(C*Sigma_filt{t+1}*C' +  R_gps);

        xfilt(:,t+1) =  xfilt(:,t+1) + K * (y - ypred);% code here
        Sigma_filt{t+1} = Sigma_filt{t+1} - K*C*Sigma_filt{t+1};% code here
        
    end
end


% plot subsampled trajectory:
colors1 = ['km']; 
map_fig_id = figure; hold on; axis([-5 25 -5 25]); axis equal; xlabel('East'); ylabel('North');
spacing = 20;
for i= [1:spacing:size(xfilt,2) size(xfilt,2)]
    plot_uncertainty_ellipse(xfilt(1:3,i), Sigma_filt{i}, map_fig_id, colors1);
end


